#include "KFApp.h"
#include "PSINS.h"

/***************************  class CCarAHRS  *********************************/
// 车载航姿仪类
// 初始化滤波器，滤波器状态包括：失准角、速度误差、位置误差、陀螺常漂、加计常偏、z轴陀螺比例系数误差
CCarAHRS::CCarAHRS() :CTDKF(16, 17) {
	levelAlignOK = yawAlignOK = initPosOK = FALSE;
    opt_zuptA = TRUE;    //是否在零速阶段修正YAW
    odo_tag = holo_tag = FALSE;    //是否有里程计量测值
	iter = -2;  ifn = 0;
	measGPSVn = measGPSPos = measINSvn = measCMvb = measMag = O31; // [0,0,0]
    measODOVN = CVect3(-111.0, -111.0, -111.0);

	// Pmax? Pmin?
//    Pmax.Set2(10.0*glv.deg, 10.0*glv.deg, 30.0*glv.deg, 50.0, 50.0, 50.0, 1.0e4 / glv.Re, 1.0e4 / glv.Re, 1.0e4,
//              1000.0*glv.dph, 1000.0*glv.dph, 1000.0*glv.dph, 100.0*glv.mg, 100.0*glv.mg, 100.0*glv.mg);
//    Pmin.Set2(1.0*glv.min, 1.0*glv.min, 10.0*glv.min, 0.01, 0.01, 0.1, 1.0 / glv.Re, 1.0 / glv.Re, 0.1,
//              0.1*glv.dph, 0.1*glv.dph, 0.1*glv.dph, 0.1*glv.mg, 0.1*glv.mg, 0.1*glv.mg);
//    Pk.SetDiag2(10.0*glv.deg, 10.0*glv.deg, 10.0*glv.deg, 1.0, 1.0, 1.0, 100.0 / glv.Re, 100.0 / glv.Re, 100.0,
//                100.0*glv.dph, 100.0*glv.dph, 100.0*glv.dph, 10.0*glv.mg, 10.0*glv.mg, 10.0*glv.mg);
//    Qt.Set2(1.1*glv.dpsh, 1.1*glv.dpsh, 1.1*glv.dpsh, 500.0*glv.ugpsHz, 500.0*glv.ugpsHz, 500.0*glv.ugpsHz, 0.0, 0.0, 0.0,
//            0.0, 0.0, 0.0, 100.0*glv.ugpsh, 100.0*glv.ugpsh, 100.0*glv.ugpsh);
//    Rk.Set2(0.5, 0.5, 0.5, 10.0 / glv.Re, 10.0 / glv.Re, 10.0, 0.01, 0.01, 0.01, 0.1 * glv.deg, 10.0 / sts, 1000.0 / sts, 1.0 / sts, 0.01, 0.05, 0.01, 1.0 * glv.deg); // 13*13
	//H(Z，X)
    //SetHk();
	measGPSYaw = 0;
}

//设置量测值
//tmeas记录当前最新的量测值更新时间
void CCarAHRS::SetMeasGPSVn(CVect3& vnGPS) {
    Hk(GPSVN, 3) = Hk(GPSVN + 1, 4) = Hk(GPSVN + 2, 5) = 1.0;
	measGPSVn = vnGPS;
	tmeas.dd[GPSVN] = tk;
}

void CCarAHRS::SetMeasGPSPos(CVect3& posGPS) {
    Hk(GPSPOS, 6) = Hk(GPSPOS + 1, 7) = Hk(GPSPOS + 2, 8) = 1.0;
	measGPSPos = posGPS;
	tmeas.dd[GPSPOS] = tk;
//	if (!initPosOK) {
//		sins.pos = posGPS;
//		initPosOK = TRUE;
//	}
}
/*带时间补偿*/
void CCarAHRS::SetMeasGPSPos(CVect3 &posGPS,const double dt) {
    auto pos = posGPS -sins.Mpv * sins.Cnb * sins.lb_gnss + sins.Mpv*sins.vn*dt;//GPS天线杆臂、时间不同步误差补偿
    SetMeasGPSPos(pos);
//    *(CVect3*)&Zk.dd[3] = sins.pos - measGPSPos;
}

void CCarAHRS::SetMeasZUPT(void) {
    Hk(ZUPT, 3) = Hk(ZUPT + 1, 4) = Hk(ZUPT + 2, 5) = 1.0;
    Hk(ZUPTYAW, 11) = 1.0;
	measINSvn = sins.vn;
	tmeas.dd[ZUPT] = tk;
}

void CCarAHRS::SetMeasMag(CVect3& mag) {
	measMag = mag;
}

void CCarAHRS::SetMeasMC(void) {
	Hk(CARMC + 0, 3) = sins.Cnb.e00; Hk(CARMC + 0, 4) = sins.Cnb.e10; Hk(CARMC + 0, 5) = sins.Cnb.e20;
	Hk(CARMC + 1, 3) = sins.Cnb.e01; Hk(CARMC + 1, 4) = sins.Cnb.e11; Hk(CARMC + 1, 5) = sins.Cnb.e21;
	Hk(CARMC + 2, 3) = sins.Cnb.e02; Hk(CARMC + 2, 4) = sins.Cnb.e12; Hk(CARMC + 2, 5) = sins.Cnb.e22;
	measCMvb = sins.vb;
	if (measCMvb.j > 20) measCMvb.j -= 20; // ？
	else if (measCMvb.j < -1) measCMvb.j -= -1;
	tmeas.dd[CARMC] = tk;
}

void CCarAHRS::SetMeasHOLO(void) {
    tmeas.dd[CARMC] = tk;
    holo_tag = TRUE;
}

void CCarAHRS::SetMeasODOVN(CVect3& odoVN) {
    measODOVN = odoVN;
    tmeas.dd[ODOVN] = tk;
    odo_tag = TRUE;
}

void CCarAHRS::SetMeasGPSYaw(double gpsYaw) {
    Hk(GPSYAW, 2) = 1.0;
	measGPSYaw = gpsYaw;
	tmeas.dd[GPSYAW] = tk;
}

//kf量测更新标志
//根据是否有量测值设置更新标志，需要更新的行对应的二进制值为1
void CCarAHRS::MeasRearrange(CSINS &sins) {
	if (yawAlignOK && measGPSVn.k != 0 && tk - tmeas.dd[GPSVN] < 0.5)   // GPSVn
	{
        measGPSVn = measGPSVn - sins._Cnb * sins.lb_gnss;
	    *(CVect3*)&Zk.dd[GPSVN] = sins.vn - measGPSVn;
		measGPSVn = O31; SetMeasFlag(0x07);    //000,0,000,000,111(二进制)
	}
	if (yawAlignOK && measGPSPos.k != 0 && tk - tmeas.dd[GPSPOS] < 0.5)   // GPSPos
	{
        measGPSPos = measGPSPos - sins.Mpv * sins.Cnb * sins.lb_gnss;
	    *(CVect3*)&Zk.dd[GPSPOS] = sins.pos - measGPSPos - sins.eth.vn2dpos(sins.vn, tk - tmeas.dd[GPSPOS] - sins.nts);
		measGPSPos = O31; SetMeasFlag(0x38);    //000,0,000,111,000
	}
	if (levelAlignOK && measINSvn.k != 0 && tk - tmeas.dd[ZUPT] < 0.1)   // ZUPT
	{
		*(CVect3*)&Zk.dd[ZUPT] = measINSvn - sins.an*(tk - tmeas.dd[ZUPT] - sins.nts);
		measINSvn = O31; SetMeasFlag(0x01C0);    //000,0,111,000,000
		if(opt_zuptA){
            Zk.dd[ZUPTYAW] = sins.wm.k / nts - 0.0;
            //sins.wm = O31;
            SetMeasFlag(0x0200);    //000,1,000,000,000
        }

	} else if (levelAlignOK && measCMvb.k != 0 && tk - tmeas.dd[CARMC]<0.1 && tk - tmeas.dd[GPSVN]>10.0)	  // CARMC
	{
		*(CVect3*)&Zk.dd[CARMC] = measCMvb;
		measCMvb = O31; SetMeasFlag(0x01C00);    //111,0,000,000,000
	} else if (levelAlignOK && holo_tag && tk - tmeas.dd[CARMC]<0.1)	  // HOLO
    {
        //cout << "HOLO:  " << tk << endl;
        CMat3 C_nv = sins.cl_Cnv();
        //速度
        Hk(CARMC + 0, 3) = C_nv.e00; Hk(CARMC + 0, 4) = C_nv.e01; Hk(CARMC + 0, 5) = C_nv.e02;
        Hk(CARMC + 2, 3) = C_nv.e20; Hk(CARMC + 2, 4) = C_nv.e21; Hk(CARMC + 2, 5) = C_nv.e22;
        //姿态
        CMat3 tmp_Matt = sins.cl_Matt();
        Hk(CARMC + 0, 0) = tmp_Matt.e00; Hk(CARMC + 0, 1) = tmp_Matt.e01; Hk(CARMC + 0, 2) = tmp_Matt.e02;
        Hk(CARMC + 2, 0) = tmp_Matt.e20; Hk(CARMC + 2, 1) = tmp_Matt.e21; Hk(CARMC + 2, 2) = tmp_Matt.e22;
        //陀螺零偏
        CMat3 tmp_Meb = sins.cl_Meb();
        Hk(CARMC + 0, 9) = tmp_Meb.e00; Hk(CARMC + 0, 10) = tmp_Meb.e01; Hk(CARMC + 0, 11) = tmp_Meb.e02;
        Hk(CARMC + 2, 9) = tmp_Meb.e20; Hk(CARMC + 2, 10) = tmp_Meb.e21; Hk(CARMC + 2, 11) = tmp_Meb.e22;

        CVect3 v_v = C_nv * sins.vn + sins.C_bv * (sins.wnb * sins.lb_odo); //括号内为叉乘，运算符重载

        Zk.dd[CARMC + 0] = v_v.i - 0.0;
        Zk.dd[CARMC + 2] = v_v.k - 0.0;
        holo_tag = FALSE; SetMeasFlag(0x01400);    //101,0,000,000,000
    }
    if (levelAlignOK && odo_tag && (measODOVN.i + 111.0) >= 0.01 && tk - tmeas.dd[ODOVN] < 0.5) {
        //构造里程计信息
        CMat3 C_nv = sins.cl_Cnv();
        //速度
        Hk(ODOVN + 0, 3) = C_nv.e00; Hk(ODOVN + 0, 4) = C_nv.e01; Hk(ODOVN + 0, 5) = C_nv.e02;
        Hk(ODOVN + 1, 3) = C_nv.e10; Hk(ODOVN + 1, 4) = C_nv.e11; Hk(ODOVN + 1, 5) = C_nv.e12;
        Hk(ODOVN + 2, 3) = C_nv.e20; Hk(ODOVN + 2, 4) = C_nv.e21; Hk(ODOVN + 2, 5) = C_nv.e22;
        //姿态
        CMat3 tmp_Matt = sins.cl_Matt();
        Hk(ODOVN + 0, 0) = tmp_Matt.e00; Hk(ODOVN + 0, 1) = tmp_Matt.e01; Hk(ODOVN + 0, 2) = tmp_Matt.e02;
        Hk(ODOVN + 1, 0) = tmp_Matt.e10; Hk(ODOVN + 1, 1) = tmp_Matt.e11; Hk(ODOVN + 1, 2) = tmp_Matt.e12;
        Hk(ODOVN + 2, 0) = tmp_Matt.e20; Hk(ODOVN + 2, 1) = tmp_Matt.e21; Hk(ODOVN + 2, 2) = tmp_Matt.e22;
        //陀螺零偏
        CMat3 tmp_Meb = sins.cl_Meb();
        Hk(ODOVN + 0, 9) = tmp_Meb.e00; Hk(ODOVN + 0, 10) = tmp_Meb.e01; Hk(ODOVN + 0, 11) = tmp_Meb.e02;
        Hk(ODOVN + 1, 9) = tmp_Meb.e10; Hk(ODOVN + 1, 10) = tmp_Meb.e11; Hk(ODOVN + 1, 11) = tmp_Meb.e12;
        Hk(ODOVN + 2, 9) = tmp_Meb.e20; Hk(ODOVN + 2, 10) = tmp_Meb.e21; Hk(ODOVN + 2, 11) = tmp_Meb.e22;
        //kd
        Hk(ODOVN + 0, 15) = measODOVN.i;
        Hk(ODOVN + 1, 15) = measODOVN.j;
        Hk(ODOVN + 2, 15) = measODOVN.k;

        CVect3 v_v = C_nv * sins.vn + sins.C_bv * (sins.wnb * sins.lb_odo); //括号内为叉乘，运算符重载
        *(CVect3*)&Zk.dd[ODOVN] = v_v - measODOVN * sins.sf_odom;
        measODOVN = CVect3(-111.0, -111.0, -111.0);
        odo_tag = FALSE;
        SetMeasFlag(0xE000);  //111,000,0,000,000,000
    }
	if (yawAlignOK && measGPSYaw != 0 && tk - tmeas.dd[GPSYAW] < 0.5)   // GPSYaw 暂时未使用
	{
		Zk.dd[GPSYAW] = sins.att.k - measGPSYaw + 2 * glv.deg;  //？
		measGPSYaw = 0;
		//		if(fabs(Zk.dd[11])<5*glv.deg)
		//			SetMeasFlag(0x1000);
	}
}

//设置P矩阵
void CCarAHRS::SetPk(CVect3 att_var, CVect3 vel_var, CVect3 pos_var, double bg_var, double ba_var, double kd_var){
    Pk.SetDiag2(att_var.i, att_var.j, att_var.k, vel_var.i, vel_var.j, vel_var.k, pos_var.i, pos_var.j, pos_var.k,
                bg_var, bg_var, bg_var, ba_var, ba_var, ba_var, kd_var);
}

//设置Q矩阵
void CCarAHRS::SetQt(double arw, double vrw, double prw, double bg_var, double bg_Tcor, double ba_var, double ba_Tcor, double kd_var, double kd_Tcor){
    Qt.Set2(arw, arw, arw, vrw, vrw, vrw, prw, prw, prw, bg_var*sqrt(2)/sqrt(bg_Tcor), bg_var*sqrt(2)/sqrt(bg_Tcor), bg_var*sqrt(2)/sqrt(bg_Tcor),
            ba_var*sqrt(2)/sqrt(ba_Tcor), ba_var*sqrt(2)/sqrt(ba_Tcor), ba_var*sqrt(2)/sqrt(ba_Tcor), kd_var*sqrt(2)/sqrt(kd_Tcor));
}

//设置R矩阵—GPSVN
void CCarAHRS::SetRk_GPSVN(CVect3 std_vn){
    *(CVect3*)&Rk.dd[GPSVN] = CVect3(std_vn.i * std_vn.i, std_vn.j * std_vn.j, std_vn.k * std_vn.k);
}

//设置R矩阵—GPSPOS
void CCarAHRS::SetRk_GPSPOS(CVect3 std_pos){
    *(CVect3*)&Rk.dd[GPSPOS] = CVect3(std_pos.i * std_pos.i, std_pos.j * std_pos.j, std_pos.k * std_pos.k);
}

//设置R矩阵—ZUPT
void CCarAHRS::SetRk_ZUPT(CVect3 var_zupt, double var_zuptA){
    *(CVect3*)&Rk.dd[ZUPT] =  CVect3(var_zupt.i * var_zupt.i, var_zupt.j * var_zupt.j, var_zupt.k * var_zupt.k);;
    Rk.dd[ZUPTYAW] = var_zuptA * var_zuptA;
}

void CCarAHRS::SetRk_ODO(CVect3 var_odo) {
    *(CVect3*)&Rk.dd[ODOVN] = CVect3(var_odo.i * var_odo.i, var_odo.j * var_odo.j, var_odo.k * var_odo.k);
}

void CCarAHRS::SetRk_CARMC(double sts) {
    *(CVect3*)&Rk.dd[CARMC] = CVect3((10.0/sts)*(10.0/sts), (1000.0/sts)*(1000.0/sts), (1.0/sts)*(1.0/sts));
}

void CCarAHRS::SetRk_HOLO(double var_holo){
    Rk.dd[CARMC + 0] = var_holo * var_holo;
    Rk.dd[CARMC + 2] = var_holo * var_holo;
}

// 初始对准——给定初始位置速度姿态
//初始的位置、速度、姿态
void CCarAHRS::Align_Static(CVect3 &pos0, CVect3 &v0, CVect3 &att0) {
    sins.pos = pos0;
    sins.vn = v0;
    sins.att = att0;
    sins.qnb = CQuat(sins.att);
}

// 初始对准——动态
//动态对准 目前只有一个大概的框架，做这部分的同学可自行修改
void CCarAHRS::Align_InMotion(CVect3 &wm, CVect3 &vm, double tk, double ts) {
    if (!levelAlignOK) {  //水平对准
        if(tk != 0){
            sins.qnb = align.Update(FALSE, &wm, &vm, 1, ts);
            if (align.tk > tk) {
                // 采用惯性系对准方法粗略找到水平姿态角
                levelAlignOK = TRUE;
            }
        }
        else{
            sins.qnb = align.Update(TRUE, &wm, &vm, 1, ts);
//            if (align.tk > tk) {
            if (normXY(measGPSVn) > 0.2) {
                // 采用惯性系对准方法粗略找到水平姿态角
                levelAlignOK = TRUE;
            }
        }

    }
    if (levelAlignOK && !yawAlignOK) {  // 方位对准
        double nm = norm(measMag);
        if (nm > 520 - 100 && nm < 520 + 250)  // 地磁
        {
            CVect3 Hn = sins.qnb * measMag;  // [XYZ, H, DEC, DIP, F] = wrldmagm(400, 34.21, 108.86, 2010)
            double nh = normXY(Hn);
            if (nh > 320 - 100 && nh < 320 + 200) {
                sins.qnb -= CVect3(0, 0, atan2(-Hn.i, -Hn.j));
                yawAlignOK = TRUE;
            }
        }
#if DEBUG_INDOOR == 1
        float th_vel = -1;
#else
        float th_vel = 1.2;
#endif
        if (normXY(measGPSVn) > th_vel ) { /** \warning debug模式中注意更改为0,避免堵塞 */   // 载车行驶速度大于5m/s
            CVect3 att(sins.qnb);
            att.k = atan2(-measGPSVn.i, measGPSVn.j);  // 利用速度计算载车行驶的航迹向，作为初始方位
            sins.qnb = CQuat(att);
            yawAlignOK = TRUE;
        }
        if (measGPSYaw != 0) {  // 有GPSYaw
            CVect3 att(sins.qnb);
            att.k = measGPSYaw;
            sins.qnb = CQuat(att);
            yawAlignOK = TRUE;
        }
    }
}

//机械编排+kf更新 机械编排部分仿照Matlab
int CCarAHRS::Update(CVect3 &wm1, CVect3 &vm1, CVect3 &wm, CVect3 &vm, double ts) {
    int res = 0;

    if (!levelAlignOK) {  //水平对准
        levelAlignOK = TRUE;
    }
    if (levelAlignOK && !yawAlignOK) {  // 方位对准
        //sins.Cnb = CMat3(sins.att);
        //sins.qnb = CQuat(sins.att);
        yawAlignOK = TRUE;
    }

    sins.Update(wm1, vm1, wm, vm, 1, ts);     //机械编排
    sins.pos_pre = sins.pos;
    res = TDUpdate(sins, ts, 0);     //kf更新

    return res;
}

//析构函数
CCarAHRS::~CCarAHRS(){
//    cout << "destructing kf" << endl;
}
